#include <joyos.h>
#include <happylib.h>
#include <math.h>

#include "define.h"
#include "monitor.h"
#include "distance.h"
#include "roboturn.h"
#include "robomove.h"

struct lock mon_lock;
int pos[3] = {0,0,0};
int8_t place = 0;

void gameMonitor(){
	int (*monitorpt)(void) = monitor;
	init_lock (&mon_lock, "Mon lock");
	create_thread(monitorpt, 1000, 150, "Monitor");
}

int monitor(void){
	/*acquire(&mon_lock);
	check_stuck = 1;
	release(&mon_lock);*/
	while(1){
		if(status == 0){ //stop or scoring
			yield();
		}else if(status == 1){
			updatePos();
			if(stuck()){
				printf("\n Stuck");
				if(motorVelNOWL > 0){
					motor_set_vel(MOTOR_PORT_LEFT, -200);
					pause(500);
				}else{
					motor_set_vel(MOTOR_PORT_LEFT, 200);
					pause(500);
				}
				if(motorVelNOWR > 0){
					motor_set_vel(MOTOR_PORT_RIGHT, -200);
					pause(500);
				}else{
					motor_set_vel(MOTOR_PORT_RIGHT, 200);
					pause(500);
				}
				motorSetVel(MOTOR_PORT_LEFT, motorVelNOWL);
				motorSetVel(MOTOR_PORT_RIGHT, motorVelNOWR);			
			}		
		}else if(status == 2){
			updateDegree();
		}
		//printf("\n x:%d y:%d deg:%d", pos[0], pos[1], pos[2]);
		pause(50);
	}
	return 0;
}
/*
void whereAmI(){
	if(initPlace == 0){ // Left
		if((pos[0] < 8 || pos[0] >= 0) &&
			(pos[1] < 0 || pos[1] > -10)){
			place = 0;
		}else if((pos[0] < 24 || pos[0] >= 8) &&
			(pos[1] < 0 || pos[1] > -10)){
			place = 1;
		}else if((pos[0] < 32 || pos[0] >= 24) &&
			(pos[1] < 0 || pos[1] > -10)){
			place = 2;
		}else if((pos[0] < 48 || pos[0] >= 32) &&
			(pos[1] < 0 || pos[1] > -10)){
			place = 3;
		}else if((pos[0] < 56 || pos[0] >= 48) &&
			(pos[1] < 0 || pos[1] > -10)){
			place = 4;
		}else if((pos[0] < 72 || pos[0] >= 56) &&
			(pos[1] < 0 || pos[1] > -10)){
			place = 5;
		}else if((pos[0] < 80 || pos[0] >= 72) &&
			(pos[1] < 0 || pos[1] > -10)){
			place = 6;
		}else if(pos[0] >= 80 &&
			(pos[1] < 0 || pos[1] > -10)){
			place = 7;
		}else if(pos[0] >= 80 &&
			(pos[1] < 0 || pos[1] > -56)){
			place = 8;
		}else if((pos[0] < 8 || pos[0] > 0) &&
			(pos[1] < 0 || pos[1] > -8)){
			place = 9;
		}else if((pos[0] < 8 || pos[0] > 0) &&
			(pos[1] < 0 || pos[1] > -8)){
			place = 10;
		}else if((pos[0] < 8 || pos[0] > 0) &&
			(pos[1] < 0 || pos[1] > -8)){
			place = 11;
		}else if((pos[0] < 8 || pos[0] > 0) &&
			(pos[1] < 0 || pos[1] > -8)){
			place = 12;
		}else if((pos[0] < 8 || pos[0] > 0) &&
			(pos[1] < 0 || pos[1] > -8)){
			place = 13;
		}else if((pos[0] < 8 || pos[0] > 0) &&
			(pos[1] < 0 || pos[1] > -8)){
			place = 14;
		}else if((pos[0] < 8 || pos[0] > 0) &&
			(pos[1] < 0 || pos[1] > -8)){
			place = 15;
		}
	}else if(initPlace == 1){  // Right
	
	}
}
*/

int shaftToDistance(int16_t shaft){ // inch
	return DIST_PER_WHEEL/SHAFT_PER_WHEEL*shaft;
}

// Start Pos = (0,0,0) (x,y,deg)
void updatePos(){ //Rely on shaft encoder
	uint16_t nowL = encoder_read(ENCODER_PORT_LEFT);
	uint16_t nowR = encoder_read(ENCODER_PORT_RIGHT);
	int delta_dist = (shaftToDistance(nowL-shaftValueMonitorL) + shaftToDistance(nowR-shaftValueMonitorR))/2;
	
	pos[0] = (delta_dist*cos(pos[2]));
	pos[1] = (delta_dist*sin(pos[2]));
}

void updateDegree(){ //Rely on gyro
	pos[2] = getCurrentAngle();
}

int *getRobotPos(){ //[0] = x [1] =y [2] = degree
	return pos;
}
/*
 * @param place
 * 0 = LEFT HOME
 * 1 = Go to 1 
 * 2 = LEFT 1
 * 3 = Go to 2
 * 4 = LEFT 2
 * 5 = Go to 4
 * 6 = LEFT 4 
 * 7 = Go to SKUNK
 * 8 = SKUNK
 * 9 = RIGHT 4
 * 10 = Go to Right 2
 * 11 = RiIGHT 2
 * 12 = Go to Right 4
 * 13 = RIGHT 4
 * 14 = Go to Right 1
 * 15 = RIGHT HOME
 */
void moveTo(int place){
	
}
void resolveUnfulfillState(){
	//myStrategy();
}

bool foundEnemy(void){
	/*double threshold = 5.5;
	double enemyDistance = getDistance();
	if (enemyDistance <= threshold)
		return true;
	else
		return false;*/
	return false;
}

bool stuck(void){
	uint16_t threshold = 10;
	uint16_t init_left_encoder_val = encoder_read(ENCODER_PORT_LEFT);
	uint16_t init_right_encoder_val = encoder_read(ENCODER_PORT_RIGHT);
	pause(1000);
	uint16_t new_left_encoder_val = encoder_read(ENCODER_PORT_LEFT);
	uint16_t new_right_encoder_val = encoder_read(ENCODER_PORT_RIGHT);
	
	if ((new_left_encoder_val - init_left_encoder_val <= threshold) ||
		(new_right_encoder_val - init_right_encoder_val <= threshold)){
		return true;
	}
	else
		return false;
} 





